#include <joyos.h>
#include <happylib.h>

// my headers
#include "umain.h"
#include "define.h"
#include "goalscore.h"
#include "robomove.h"
#include "roboturn.h"
#include "led.h"
#include "bumper.h"
#include "distance.h"
#include "init.h"
#include "skunk.h"
#include "strategy.h"
#include "robotest.h"
#include "fantasy.h"
#include "monitor.h"

/*************************************************************
 * TESTING AREA
 */


int usetup(void) {
	openSkunk();
	servo_set_pos(SERVO_SCORE, 240);

	caliUnderLed();
	caliLeftLed();
	caliRightLed();
	printf("\nCalibrate Gyro");
	go_click();
	calibrateGyro();
	set_auto_halt(0);
	return 0;
}

int umain (void) {
	printf("\nTurning with PatTurn");
	patTurn(90, 200);
	patTurn(-90, 200);
	patTurn(180, 200);
	return(0);
	//roboTest();
/*	while(1){
		turnWithShaft(90);
				go_click();
		turnWithShaft(180);
				go_click();
						turnWithShaft(-90);		
		turnWithShaft(-180);
		go_click();
		turnWithShaft(-90);		
				go_click();
	}*/
	pointToDo[0] = 4;
	pointToDo[1] = 4;
	pointToDo[2] = 4;
	/*
	initPlace = 0;
	initOrient = 2;
	currentOrient = initOrient;*/
	
	
	initPose();
	printf_P (PSTR("\nPlace %d, orient %d"), initPlace, initOrient);
	pause(1000);
	initTurn();
	
	state = 1;
	myStrategy();
	
	//
	//state = 1;
	
	//printf_P (PSTR("\nClick to Rock!"));
	//go_click();
	
	//myStrategy();
	
	return 0;
}



// Maybe implement to ratioCalibrate in usetup
void ratioTest(){
	double ratio[10] ; 
	double sum = 0;
	roboStraight(200);
	for(int i =0; i< 10 ; i++){
		encoder_reset(ENCODER_PORT_LEFT);
		encoder_reset(ENCODER_PORT_RIGHT);		
		pause(1000);
		ratio[i]=encoder_read(ENCODER_PORT_LEFT)/encoder_read(ENCODER_PORT_RIGHT);	
		sum+=ratio[i];
	}
	printf("\nFinish>>ratio: %f",(double)sum/10);
	go_click();
}
/*
// Entry point to contestant code.
// Create threads and return 0.
int umain (void) {
	//fantasy(); >> Test passed as of yesterday
	
	//gameMonitor(); Test This and action too for Stuck Handler
	//action();
	
	//Run this to test Left Right Ratio of the Wheel
	ratioTest();
	return 0;
}

*/

/*************************************************************
 * REAL CODE
 */

// usetup is called during the calibration period. It must return before the
// period ends.
/*

int usetup (void) {
	printf_P (PSTR("\nHello My Boss!"));
	go_click();
	
	// Point Input
	getGoalScore(); // also modify pointToDo
	printf_P (PSTR("\nScore Set: %d"), goalPoint);
	go_click();
	
	// Calibrate Goal and Skunk
	//calibrateGoalServo();
	//pause(1000);
	//calibrateSkunkServo();
	//pause(1000);
	
	// Under IR/LED
	caliUnderLed();
	pause(2000);
	caliLeftLed();
	//pause(1000);
	
	caliRightLed();
	//pause(1000);
	
	// Gyro
	printf_P (PSTR("\nCalibrate Gyro"));	
	go_click ();
	calibrateGyro();
	
	return 0;
}

int umain (void) {
	//fantasy();
	//roboTest();
	
	
	action();

	//initPlace = 1;
	//initOrient = 1;
	//currentOrient = initOrient;
	//state = 1;
	//myStrategy();

	return 0;
}
	

void action(void){
	
	initPose();
	printf_P (PSTR("\nPlace %d, orient %d"), initPlace, initOrient);
	initTurn();
	
	state = 1;
	myStrategy();
}
*/
